#include "pathBuilder.hpp"
#include "voxeldata.pb.h"

#include <algorithm>
#include <unordered_map>
#include <cstring>
#include <iostream>
#include <fstream>
#include <glm/gtc/type_ptr.hpp>

constexpr uint32_t VALID_LEAF_MASK = FLAG_LEAF | FLAG_VALIDVOXEL;

glm::vec3 L_FindShortestLinkPt( SpaceRoot& root, uint32_t ci, uint32_t ni, const glm::vec3& s, const glm::vec3& e, const float sa )
{
    SpaceNode* sn = root.nodes + ci;
    SpaceNode* nn = root.nodes + ni;
    if( sn->voxel.size > nn->voxel.size )
    {
        SpaceNode* t = nn;
        nn = sn;
        sn = t;
    }

    glm::vec3 le = e - sn->voxel.center;
    glm::vec3 ls = s - sn->voxel.center;
    
    float hf = sn->voxel.size * 0.5f;
    float md = (sn->voxel.size + nn->voxel.size) * 0.5f;
    glm::vec3 diff = nn->voxel.center - sn->voxel.center;
    glm::vec3 locPt;
    locPt = le;
    if( md - glm::abs( diff.y ) < 0.00001f )
    {
        // up or down;
        locPt.y = diff.y > 0.0f ? hf : -hf;
        if( le.y != ls.y ){
            float t = (locPt.y - ls.y) / (le.y - ls.y);
            if( t >= 0.0f && t <= 1.0f )
            {
                locPt.x = (le.x - ls.x) * t + ls.x;
                locPt.z = (le.z - ls.z) * t + ls.z;
            }
        }       
    } else if( md - glm::abs( diff.x ) < 0.00001f )
    {
        // left or right
        locPt.x = diff.x > 0.0f ? hf : -hf;
        if( le.x != ls.x ){
            float t = (locPt.x - ls.x) / (le.x - ls.x);
            if( t >= 0.0f && t <= 1.0f )
            {
                locPt.y = (le.y - ls.y) * t + ls.y;
                locPt.z = (le.z - ls.z) * t + ls.z;
            }
        }  
    } else if( md - glm::abs( diff.z ) < 0.00001f )
    {
        // front or back
        locPt.z = diff.z > 0.0f ? hf : -hf;
        if( le.z != ls.z ){
            float t = (locPt.z - ls.z) / (le.z - ls.z);
            if( t >= 0.0f && t <= 1.0f )
            {
                locPt.x = (le.x - ls.x) * t + ls.x;
                locPt.y = (le.y - ls.y) * t + ls.y;
            }
        }
    } 
    const glm::vec3 min = glm::min( glm::vec3(), glm::vec3(1.0f) * (-hf + sa) );
    const glm::vec3 max = glm::max( glm::vec3(), glm::vec3(1.0f) * (hf - sa) );
    // locPt = glm::clamp(locPt, glm::vec3(-hf, -hf, -hf) + 2.0f * sa, glm::vec3(hf, hf, hf) - 2.0f * sa);
    locPt = glm::clamp( locPt, min, max );
    return sn->voxel.center + locPt;
}

uint32_t L_FetchNodeIndex( int x, int y, int z, uint32_t xc, uint32_t yc, uint32_t zc )
{
    if( x < 0 || y < 0 || z < 0 || (uint32_t)x >= xc || (uint32_t)y >= yc || (uint32_t)z >= zc ) return UINT32_MAX;
    return x * ( yc * zc ) + y * zc + z;
}

BuilderNode* L_FetchProxyNode( BuilderNode* p, const uint32_t dir, const uint32_t sec )
{
    if( p == nullptr || p->proxy[dir] == nullptr ) return nullptr;
    if( p->proxy[dir]->children[sec] == nullptr ) return p->proxy[dir];
    return p->proxy[dir]->children[sec];
}

inline uint32_t L_MapPosToOctreeIndex( const glm::vec3 pos, const glm::vec3 voxlCenter )
{
    //       3 --- 7
    //    2 --- 6  |
    //    |  1 -|- 5
    //    0 --- 4
    uint32_t sec = 0;
    glm::vec3 np = pos - voxlCenter;
    if( np.x < 0.0f )
    {
        if( np.y < 0.0f ){ np.z < 0.0f ? sec = 0 : sec = 1; }
        else { np.z < 0.0f ? sec = 2 : sec = 3; }
    } else {
        if( np.y < 0.0f ){ np.z < 0.0f ? sec = 4 : sec = 5; }
        else { np.z < 0.0f ? sec = 6 : sec = 7; }
    }
    return sec;
}

inline void L_SetupBuilderProxies( BuilderNode* cur, const uint16_t sector )
{
    //       3 --- 7
    //    2 --- 6  |
    //    |  1 -|- 5
    //    0 --- 4
    // up, down, left, right, front, back
    BuilderNode* p = cur->parent;
    switch (sector)
    {
    case 0:
        {
            cur->proxy[0] = p->children[2];
            cur->proxy[1] = L_FetchProxyNode(p, 1, 2);
            cur->proxy[2] = L_FetchProxyNode(p, 2, 4);
            cur->proxy[3] = p->children[4];
            cur->proxy[4] = L_FetchProxyNode(p, 4, 1);
            cur->proxy[5] = p->children[1]; 
        }
        break;
    case 1:
        {
            cur->proxy[0] = p->children[3];
            cur->proxy[1] = L_FetchProxyNode(p, 1, 3);
            cur->proxy[2] = L_FetchProxyNode(p, 2, 5);
            cur->proxy[3] = p->children[5];
            cur->proxy[4] = p->children[0];
            cur->proxy[5] = L_FetchProxyNode(p, 5, 0);
        }
        break;
    case 2:
        {
            cur->proxy[0] = L_FetchProxyNode(p, 0, 0);
            cur->proxy[1] = p->children[0];
            cur->proxy[2] = L_FetchProxyNode(p, 2, 6);
            cur->proxy[3] = p->children[6];
            cur->proxy[4] = L_FetchProxyNode(p, 4, 3);
            cur->proxy[5] = p->children[3];
        }
        break;
    case 3:
        {
            cur->proxy[0] = L_FetchProxyNode(p, 0, 1);
            cur->proxy[1] = p->children[1];
            cur->proxy[2] = L_FetchProxyNode(p, 2, 7);
            cur->proxy[3] = p->children[7];
            cur->proxy[4] = p->children[2];
            cur->proxy[5] = L_FetchProxyNode(p, 5, 2);
        }
        break;
    case 4:
        {
            cur->proxy[0] = p->children[6];
            cur->proxy[1] = L_FetchProxyNode(p, 1, 6); 
            cur->proxy[2] = p->children[0];
            cur->proxy[3] = L_FetchProxyNode(p, 3, 0);
            cur->proxy[4] = L_FetchProxyNode(p, 4, 5);
            cur->proxy[5] = p->children[5];
        }
        break;
    case 5:
        {
            cur->proxy[0] = p->children[7];
            cur->proxy[1] = L_FetchProxyNode(p, 1, 7); 
            cur->proxy[2] = p->children[1];
            cur->proxy[3] = L_FetchProxyNode(p, 3, 1);
            cur->proxy[4] = p->children[4];
            cur->proxy[5] = L_FetchProxyNode(p, 5, 4);
        }
        break;
    case 6:
        {
            cur->proxy[0] = L_FetchProxyNode(p, 0, 4);
            cur->proxy[1] = p->children[4];
            cur->proxy[2] = p->children[2];
            cur->proxy[3] = L_FetchProxyNode(p, 3, 2);
            cur->proxy[4] = L_FetchProxyNode(p, 4, 7);
            cur->proxy[5] = p->children[7];
        }
        break;
    case 7:
        {
            cur->proxy[0] = L_FetchProxyNode(p, 0, 5);
            cur->proxy[1] = p->children[5];
            cur->proxy[2] = p->children[3];
            cur->proxy[3] = L_FetchProxyNode(p, 3, 3);
            cur->proxy[4] = p->children[6];
            cur->proxy[5] = L_FetchProxyNode(p, 5, 6);
        }
        break;
    }
}

inline bool L_BuilderConnectionLess( const BuilderNodeConnection& op1, const BuilderNodeConnection& op2 )
{
    if( op1.v0->index == op2.v0->index )
    {
        if( op1.v1->index == op2.v1->index ) return true;
        else return op1.v1->index < op2.v1->index ? true : false;
    }
    return op1.v0->index < op2.v0->index ? true : false;
}

inline void L_AddConnection( BuilderNode* v0, BuilderNode* v1, DynamicArray<BuilderNodeConnection>& res )
{
    if( v0 == nullptr || v1 == nullptr ) return;
    res.Add({ v0, v1 });
    res.Add({ v1, v0 });
}

inline void L_DedupConnections( DynamicArray<BuilderNodeConnection>& res )
{
    if( res.Size() == 0 ) return;
    std::sort( res.Data(), res.Data() + res.Size(), L_BuilderConnectionLess );
    uint32_t vi = 0;
    for( uint32_t i = 0; i < res.Size(); ++i )
    {
        const BuilderNodeConnection& p0 = res[vi];
        const BuilderNodeConnection& p1 = res[i];
        if( memcmp( &p0, &p1, sizeof(BuilderNodeConnection) ) != 0 )
        {
            vi += 1;
            res[vi] = res[i];
        }
    }
    res.SetSize(vi + 1);
}

void SetupBuilderProxies( BuilderRoot& root )
{
    uint32_t xc = root.dimesions[0];
    uint32_t yc = root.dimesions[1];
    uint32_t zc = root.dimesions[2];

    for( uint32_t x = 0; x < xc; ++x )
    {
        for( uint32_t y = 0; y < yc; ++y )
        {
            for( uint32_t z = 0; z < zc; ++z )
            {
                uint32_t ci = x * ( yc * zc ) + y * zc + z;
                BuilderNode* cur = root.children[ci];
                // up, down, left, right, front, back
                if( cur != nullptr )
                {
                    uint32_t ii[6];
                    ii[0] = L_FetchNodeIndex( x, y + 1, z, xc, yc, zc );
                    ii[1] = L_FetchNodeIndex( x, y - 1, z, xc, yc, zc );
                    ii[2] = L_FetchNodeIndex( x - 1, y, z, xc, yc, zc );
                    ii[3] = L_FetchNodeIndex( x + 1, y, z, xc, yc, zc );
                    ii[4] = L_FetchNodeIndex( x, y, z - 1, xc, yc, zc );
                    ii[5] = L_FetchNodeIndex( x, y, z + 1, xc, yc, zc );
                    for( uint32_t j = 0; j < 6; ++j )
                    {
                        cur->proxy[j] = ii[j] == UINT32_MAX ? nullptr : root.children[ii[j]];
                    }
                }
            }
        }
    }

    CircularDeque<BuilderNode*> processList;
    for( uint32_t i = 0; i < root.childrenCnt; ++i )
    { if( root.children[i] != nullptr ){ processList.PushBack( root.children[i] );} }

    while( processList.Size() != 0 )
    { 
        BuilderNode* r = processList[0];
        processList.PopFront();
        for( uint32_t i = 0; i < 8; ++i )
        {
            BuilderNode* cc = r->children[i];
            if( cc != nullptr && (cc->mask & FLAG_VALIDVOXEL) == FLAG_VALIDVOXEL )
            {
                L_SetupBuilderProxies( cc, i );
                processList.PushBack( cc );
            }
        }
    }
}

uint32_t FindBuilderRootCnt( BuilderRoot& root )
{
    uint32_t res = 0;
    for( uint32_t i = 0; i < root.childrenCnt; ++i )
    { FindBuilderNodeCnt( root.children[i], res ); }
    return res;
}

void FindBuilderNodeCnt( BuilderNode* tree, uint32_t& res )
{
    if( tree == nullptr ) return;
    for( uint32_t i = 0; i < 8; ++i )
    { FindBuilderNodeCnt( tree->children[i], res ); }
    res += 1;
}

void ListBuilderRoot( BuilderRoot& root, BuilderNode** res, bool updateIndex )
{
    uint32_t iter = 0;
    for( uint32_t i = 0; i < root.childrenCnt; ++i )
    { ListBuilderTree( root.children[i], res, iter, updateIndex ); }
}

void ListBuilderTree( BuilderNode* tree, BuilderNode** res, uint32_t& iter, bool updateIndex )
{
    if( tree == nullptr ) return;
    auto func = [&]( BuilderNode* cur )
    {
        if(updateIndex){ cur->index = iter;}
        res[iter] = cur;
        iter += 1;
    };
    Foreach(tree, func);
}

void CreateBuilderRoot( BuilderRoot& root, glm::vec3 center, glm::vec3 volume, float minsize, ISpaceBuilderHandler* hanlder )
{
    uint32_t index = 0;
    float minEdge = std::numeric_limits<float>::max();
    for( uint32_t i = 0; i < 3; ++i )
    {
        if( volume[i] < minEdge ){ minEdge = volume[i]; }
    }
    
    uint32_t xc = glm::ceil(volume.x / minEdge);
    uint32_t yc = glm::ceil(volume.y / minEdge);
    uint32_t zc = glm::ceil(volume.z / minEdge);

    DeleteBuilderRoot( root );
    root.childrenCnt = xc * yc * zc;
    root.children = MemoryPool::AllocArray<BuilderNode*>( root.childrenCnt );
    root.size = minEdge;
    root.center = center;
    root.dimesions[0] = xc;
    root.dimesions[1] = yc;
    root.dimesions[2] = zc;

    glm::vec3 size = glm::vec3( minEdge, minEdge, minEdge );
    glm::vec3 sPos = center - volume / 2.0f;

    uint32_t i = 0;
    for(uint32_t ix = 0; ix < xc; ++ix )
    {
        for(uint32_t iy = 0; iy < yc; ++iy )
        {
            for( uint32_t iz = 0; iz < zc; ++iz )
            {
                BuilderNode* nv = new BuilderNode();
                memset( nv->children, 0, sizeof(BuilderNode::children));
                memset( nv->proxy, 0, sizeof(BuilderNode::proxy));
                nv->parent = nullptr;
                nv->index = index;
                nv->mask = 0;
                nv->voxel.center = sPos + glm::vec3( ix, iy, iz ) * minEdge + size * 0.5f;
                nv->voxel.size = minEdge;
                root.children[i] = nv;
                index += 1;
                i += 1;
            }
        }
    }

    for( uint32_t i = 0; i < root.childrenCnt; ++i )
    { CreateBuilderTree( root.children[i], minsize, index, hanlder ); }
}

void CreateBuilderTree( BuilderNode* tar, float minsize, uint32_t& index, ISpaceBuilderHandler* cond )
{
    bool valid = cond->ValidCond(tar);
    if( tar->voxel.size > minsize && !valid )
    {
        cond->SplitCallback( valid, tar );
        float newSize = tar->voxel.size * 0.5f;
        glm::vec3 sp = tar->voxel.center - glm::vec3( newSize, newSize, newSize );
        for( uint32_t x = 0; x < 2; ++x )
        {
            for( uint32_t y = 0; y < 2; ++y )
            {
                for( uint32_t z = 0; z < 2; ++z )
                {
                    BuilderNode* nv = new BuilderNode();
                    memset( nv->children, 0, sizeof(BuilderNode::children));
                    memset( nv->proxy, 0, sizeof(BuilderNode::proxy));
                    nv->parent = tar;
                    nv->index = index;
                    nv->mask = 0;
                    nv->voxel.center = sp + glm::vec3( x, y, z ) * newSize + newSize * 0.5f;
                    nv->voxel.size = newSize;
                    index += 1;
                    tar->children[x * 4 + y * 2 + z] = nv;
                    CreateBuilderTree( nv, minsize, index, cond );
                }
            }
        }
    } else {
        if ( valid )
        {
            BuilderNode* cur = tar;
            while( cur != nullptr && ( cur->mask & FLAG_VALIDVOXEL ) == 0 )
            {
                cur->mask = cur->mask | FLAG_VALIDVOXEL;
                cur = tar->parent;
            }
        }
        cond->LeafCallback( valid, tar );
        tar->mask = tar->mask | FLAG_LEAF;
    }
}

void PruneBuilderRoot( BuilderRoot& root, bool(*pruneFunc)(const BuilderNode* tar) )
{
    for(uint32_t i = 0; i < root.childrenCnt; ++i )
    {
        if( root.children[i] != nullptr )
        {
            if( pruneFunc( root.children[i] ) )
            {
                DeleteBuilderTree( root.children[i] );
                root.children[i] = nullptr;
            } else {
                PruneBuilderTree( root.children[i], pruneFunc );
            }
        }
    }
}

void PruneBuilderTree( BuilderNode* tree, bool(*pruneFunc)(const BuilderNode* tar) )
{
    if( tree == nullptr ) return;
    auto doFunc = [&]( BuilderNode* cur )
    {
        for(uint32_t i = 0; i < 8; ++i )
        {
            if( cur->children[i] != nullptr && pruneFunc(cur->children[i]) )
            {
                DeleteBuilderTree( cur->children[i] );
                cur->children[i] = nullptr;
            }
        }
    };
    Foreach( tree, doFunc );
}

void SetupBuilderConnections( BuilderRoot& root )
{
    DynamicArray<BuilderNodeConnection> connections(32);
    auto doFunc = [&](BuilderNode* cur)
    {
        if( (cur->mask & VALID_LEAF_MASK) == VALID_LEAF_MASK )
        {
            for( uint32_t i = 0; i < 6; ++i )
            {
                BuilderNode* p = cur->proxy[i];
                if( p != nullptr && (p->mask & VALID_LEAF_MASK) == VALID_LEAF_MASK )
                { L_AddConnection( cur, p, connections ); }
            }
        }
    };
    for( uint32_t i = 0; i < root.childrenCnt; ++i )
    { if( root.children[i] != nullptr ){ Foreach(root.children[i], doFunc); } }

    L_DedupConnections( connections );
    
    if( connections.Size() > 0 )
    {
        for( uint32_t i = 0; i < connections.Size(); ++i )
        {
            uint32_t ci = i;
            while( i + 1 < connections.Size() && connections[i + 1].v0 == connections[ci].v0 )
            { i += 1; }
            BuilderNode* cur = connections[ci].v0;
            cur->connectionCnt = i - ci + 1;
            cur->connections = MemoryPool::AllocArray<BuilderNode*>( cur->connectionCnt );
            for( uint32_t j = 0; j < cur->connectionCnt; ++j )
            { cur->connections[j] = connections[ci + j].v1; }
        }
    }
}

void SelectBuilderArea( BuilderRoot& root, glm::vec3 selectPt )
{
    auto selectFunc = []( const BuilderNode* cur ){ return (cur->mask & VALID_LEAF_MASK) == VALID_LEAF_MASK; };
    BuilderNode* selectNode = SearchBuilderRoot( selectPt, root, selectFunc );
    if( selectNode != nullptr )
    {
        selectNode->mask = selectNode->mask | FLAG_SELECTED;
        DynamicArray<BuilderNode*> stack;
        stack.Add( selectNode );
        while( !stack.Empty() )
        {
            BuilderNode* cur = stack.Back();
            stack.Remove();
            for( uint32_t i = 0; i < cur->connectionCnt; ++i )
            {
                BuilderNode* con = cur->connections[i];
                if( con != nullptr && (con->mask & FLAG_SELECTED) == 0 )
                {
                    con->mask = con->mask | FLAG_SELECTED;
                    stack.Add( con );
                }
            }
            cur = cur->parent;
            while( cur != nullptr && (cur->mask & FLAG_SELECTED) == 0 )
            {
                cur->mask = cur->mask | FLAG_SELECTED;
                cur = cur->parent;
            }
        }
    }

    auto pruneFunc = []( const BuilderNode* cur ){ return (cur->mask & FLAG_SELECTED) == 0; };
    PruneBuilderRoot( root, pruneFunc );
}

void DeleteBuilderRoot( BuilderRoot& root )
{
    for( uint32_t i = 0; i < root.childrenCnt; ++i )
    { DeleteBuilderTree( root.children[i] ); }
    if( root.children != nullptr ){ MemoryPool::DeallocArray<BuilderNode*>( root.children, root.childrenCnt ); }
    root.childrenCnt = 0;
    root.children = nullptr;
}

void DeleteBuilderTree( BuilderNode* tar )
{
    if( tar == nullptr ) return;
    for( uint32_t i = 0; i < 8; ++i )
    { if( tar->children[i] != nullptr ){ DeleteBuilderTree( tar->children[i] ); } }
    if( tar->connections != nullptr ){ MemoryPool::DeallocArray<BuilderNode*>( tar->connections, tar->connectionCnt ); }
    tar->connectionCnt = 0;
    tar->connections = nullptr;
    delete tar;
}

BuilderNode* SearchBuilderNode( glm::vec3 pos, BuilderNode* cur, bool(*leafFunc)(const BuilderNode* tar) )
{
    if( (cur->mask & FLAG_LEAF) != 0 )
    {
        return leafFunc(cur) ? cur : nullptr;
    } else {
        uint32_t sec = L_MapPosToOctreeIndex( pos, cur->voxel.center );
        BuilderNode* secVol = cur->children[sec];
        return secVol != nullptr ? SearchBuilderNode(pos, secVol, leafFunc) : nullptr;
    }
}

BuilderNode* SearchBuilderRoot( glm::vec3 pos, BuilderRoot& root, bool(*leafFunc)(const BuilderNode* tar) )
{
    glm::vec3 vs = glm::vec3( root.dimesions[0], root.dimesions[1], root.dimesions[2] ) * root.size;
    glm::vec3 origin = root.center - vs * 0.5f;
    glm::vec3 np = pos - origin;
    int x = glm::floor(np.x / root.size);
    int y = glm::floor(np.y / root.size);
    int z = glm::floor(np.z / root.size);

    uint32_t index = L_FetchNodeIndex(x, y, z, root.dimesions[0], root.dimesions[1], root.dimesions[2] );
    if( index == UINT32_MAX ) return nullptr;
    BuilderNode* vol = root.children[index];
    if( vol == nullptr ) return vol;
    else return SearchBuilderNode( pos, vol, leafFunc );
}

void BuildSpaceRoot( BuilderRoot& root, SpaceRoot& res )
{
    uint32_t nodeCnt = 0;
    uint32_t connectionCnt = 0;

    auto doFunc = [&]( BuilderNode* cur )
    {
        nodeCnt += 1;
        connectionCnt += cur->connectionCnt;
    };
    for( uint32_t i = 0; i < root.childrenCnt; ++i )
    { Foreach( root.children[i], doFunc ); }
    BuilderNode* list[nodeCnt];
    ListBuilderRoot( root, list, true );

    res.size = root.size;
    memcpy( res.dimesions, root.dimesions, sizeof( res.dimesions ) );
    res.center = root.center;
    res.nodeCnt = nodeCnt;
    res.nodes = new SpaceNode[ nodeCnt ];
    res.connectionCnt = connectionCnt;
    res.connections = new uint32_t[ connectionCnt ];
    res.childrenCnt = root.childrenCnt;
    res.children = new uint32_t[ res.childrenCnt ];

    uint32_t connectionIndex = 0;
    for( uint32_t i = 0; i < nodeCnt; ++i )
    {
        SpaceNode& c = res.nodes[i];
        BuilderNode* n = list[i];
        c.voxel = n->voxel;
        c.mask = (n->mask & FLAG_LEAF);
        c.parent = n->parent == nullptr ? UINT32_MAX : n->parent->index;
        for( uint32_t j = 0; j < 8; ++j )
        { c.children[j] = n->children[j] == nullptr ? UINT32_MAX : n->children[j]->index; }
        c.connectionCnt = n->connectionCnt;
        c.connections = res.connections + connectionIndex;
        for( uint32_t j = 0; j < c.connectionCnt; ++j )
        { c.connections[j] = n->connections[j]->index; }
        connectionIndex += c.connectionCnt;        
    }
    for( uint32_t i = 0; i < res.childrenCnt; ++i )
    { res.children[i] = root.children[i] == nullptr ? UINT32_MAX : root.children[i]->index; }
}

SpaceNode* SearchSpaceNode( glm::vec3 pos, SpaceNode* nList, uint32_t offset, bool(*leafFunc)( const SpaceNode* tar) )
{
    SpaceNode* cur = nList + offset;
    if( (cur->mask & FLAG_LEAF) != 0 )
    {
        return leafFunc(cur) ? cur : nullptr;
    } else {
        const uint32_t sec = L_MapPosToOctreeIndex( pos, cur->voxel.center );
        const uint32_t ni = cur->children[sec];
        return ni != UINT32_MAX ? SearchSpaceNode( pos, nList, ni, leafFunc) : nullptr;
    }
}

SpaceNode* SearchSpaceRoot( glm::vec3 pos, SpaceRoot& root, bool(*leafFunc)( const SpaceNode* tar) )
{
    glm::vec3 vs = glm::vec3( root.dimesions[0], root.dimesions[1], root.dimesions[2] ) * root.size;
    glm::vec3 origin = root.center - vs * 0.5f;
    glm::vec3 np = pos - origin;
    int x = glm::floor(np.x / root.size);
    int y = glm::floor(np.y / root.size);
    int z = glm::floor(np.z / root.size);

    uint32_t index = L_FetchNodeIndex(x, y, z, root.dimesions[0], root.dimesions[1], root.dimesions[2] );
    if( index == UINT32_MAX ) return nullptr;
    uint32_t nIndex = root.children[index];
    if( nIndex == UINT32_MAX ) return nullptr;
    else return SearchSpaceNode( pos, root.nodes, nIndex, leafFunc );
}

void DeleteSpaceRoot( SpaceRoot& root )
{
    if( root.nodes != nullptr ){  delete [] root.nodes; }
    if( root.connections != nullptr ){ delete [] root.connections; }
    if( root.children != nullptr ){ delete [] root.children; }
    root.childrenCnt = 0;
    root.nodeCnt = 0;
    root.connectionCnt = 0;
}

void SerializeSpaceInfo(  const SpaceRoot& root, char*& res, uint32_t& size )
{
    PB_SpaceRoot datum;

    datum.set_size( root.size );
    datum.mutable_dimensions()->Assign( root.dimesions, root.dimesions + 3 );
    const float* center = glm::value_ptr( root.center );
    datum.mutable_center()->Assign( center, center + 3 );
    datum.mutable_children()->Assign( root.children, root.children + root.childrenCnt );
    datum.mutable_connections()->Assign(root.connections, root.connections + root.connectionCnt);
    for(uint32_t i = 0; i < root.nodeCnt; ++i )
    {
        PB_SpaceNode* n = datum.mutable_nodes()->Add();
        const SpaceNode* src = root.nodes + i;
        const float* cptr = glm::value_ptr( src->voxel.center );
        
        PB_Voxel* v = n->mutable_voxel();
        v->set_size( src->voxel.size );
        v->mutable_center()->Assign( cptr, cptr + 3 );

        n->set_mask( src->mask );
        n->set_parent( src->parent );
        n->set_connection_cnt( src->connectionCnt );
        n->set_connections( src->connections - root.connections );
        n->mutable_children()->Assign( src->children, src->children + 8 );
    }

    size = datum.ByteSizeLong();
    res = new char[size];
    datum.SerializeToArray( res, size );
}

void DeserializeSpaceInfo( SpaceRoot& root, const char* res, const uint32_t size )
{
    PB_SpaceRoot datum;
    datum.ParseFromArray( res, size );
    
    root.size = datum.size();
    root.center = glm::make_vec3( datum.center().data() );
    memcpy( root.dimesions, datum.dimensions().data(), 3 * sizeof(uint32_t) );    
    root.childrenCnt = datum.children_size();
    root.children = new uint32_t[root.childrenCnt];
    memcpy( root.children, datum.children().data(), root.childrenCnt * sizeof(uint32_t) );

    root.nodeCnt = datum.nodes_size();
    root.nodes = new SpaceNode[root.nodeCnt];
    root.connectionCnt = datum.connections_size();
    root.connections = new uint32_t[root.connectionCnt];
    memcpy( root.connections, datum.connections().data(), root.connectionCnt * sizeof(uint32_t) );

    for( uint32_t i = 0; i < root.nodeCnt; ++i )
    {
        const PB_SpaceNode& nodeSrc = datum.nodes()[i];
        SpaceNode& nodeTar = root.nodes[i];

        nodeTar.voxel.size = nodeSrc.voxel().size();
        nodeTar.voxel.center = glm::make_vec3( nodeSrc.voxel().center().data() );
        nodeTar.mask = nodeSrc.mask();
        nodeTar.parent = nodeSrc.parent();
        nodeTar.connectionCnt = nodeSrc.connection_cnt();
        nodeTar.connections = root.connections + nodeSrc.connections();
        memcpy( nodeTar.children, nodeSrc.children().data(), 8 * sizeof(uint32_t) );
    }
}

int ExportToFile( const char* path, const SpaceRoot& root )
{
    std::ofstream file;
    file.open( path, std::ios::out | std::ios::binary );

    if( file.is_open() )
    {
        const uint32_t hstr[] = FILE_MAGIC;
        char* datum = nullptr;
        uint32_t datumSize;
        SerializeSpaceInfo( root, datum, datumSize );

        file.write( (char*)hstr, sizeof(hstr) );
        file.write( (char*)&datumSize, sizeof(uint32_t) );
        file.write( datum, datumSize );
        file.close();
        delete [] datum;
        return 0;
    }
    return 1;
}

int ImportFromFile( const char* path, SpaceRoot& root )
{
    std::ifstream file;
    file.open( path, std::ios::in | std::ios::binary );
    if( file.is_open() )
    {
        int res = 0;
        const uint32_t hstr[] = FILE_MAGIC;
        uint32_t rhstr[2];

        file.seekg( std::ios::beg );
        file.read( (char*)rhstr, sizeof(hstr) );
    
        if( memcmp( rhstr, hstr, sizeof(hstr) ) != 0 )
        { res = 2; }
        else { 
            uint32_t datumSize;
            file.read( (char*)&datumSize, sizeof(uint32_t) );
            char* datum = new char[datumSize];
            file.read( datum, datumSize );
            DeserializeSpaceInfo( root, datum, datumSize );
            delete [] datum;
        }

        file.close();
        return res;
    } else {
        std::string what = std::string("Failed to read space info file: ") + path;
        throw std::invalid_argument( what );
    }
    return 1;
}

void FindPathInSpaceRoot( SpaceRoot& root, glm::vec3 s, glm::vec3 e, float sa, CircularDeque<glm::vec3>& wps,
    bool (*lfunc) ( const SpaceNode*), float (*hfunc) (SpaceNode*, glm::vec3) )
{
    SpaceNode* sn = SearchSpaceRoot( s, root, lfunc );
    SpaceNode* en = SearchSpaceRoot( e, root, lfunc );

    wps.Clear();
    if( sn == nullptr || en == nullptr ) return;

    DynamicArray<uint32_t> nStack;
    QuickSearch( root, sn - root.nodes, en - root.nodes, nStack );
    if( nStack.Empty() )
    { AStarSearch( root, sn - root.nodes, en - root.nodes, e, nStack, lfunc, hfunc ); }
    BuildPathFromVoxelList( nStack, root, s, e, sa, wps );
}

void BuildPathFromVoxelList( DynamicArray<uint32_t>& nStack, SpaceRoot& root, glm::vec3 s, glm::vec3 e, float sa,
    CircularDeque<glm::vec3>& wps )
{
    if( nStack.Empty() ) return;

    wps.PushBack(s);
    while( nStack.Size() > 1 )
    {
        uint32_t si = nStack.Back();
        nStack.Remove();
        uint32_t ni = nStack.Back();
        const glm::vec3 cs = wps.Back();
        wps.PushBack( L_FindShortestLinkPt( root, si, ni, cs, e, sa ) );
    }

    if( nStack.Size() == 1 )
    {
        nStack.Remove();
        wps.PushBack(e);
    }
}

void QuickSearch( SpaceRoot& root, uint32_t si, uint32_t ei, DynamicArray<uint32_t>& wps  )
{
    if( si == ei )
    {
        wps.Add( si );
    } else {
        SpaceNode* sn = root.nodes + si;
        bool found = false;
        for( uint32_t i = 0; !found && i < sn->connectionCnt; ++i )
        {
            uint32_t cei = sn->connections[i];
            found = cei == ei;
            if( found )
            {
                wps.Add( si);
                wps.Add( cei );
            }
        }
    }
}

void AStarSearch( SpaceRoot& root, uint32_t si, uint32_t ei, glm::vec3 e, DynamicArray<uint32_t>& wps,
    bool (*lfunc) ( const SpaceNode*), float (*hfunc) (SpaceNode*, glm::vec3) )
{
    AStarSearchNode searchInfo[ root.nodeCnt ];
    for( uint32_t i = 0; i < root.nodeCnt; ++i )
    { searchInfo[i] = AStarSearchNode(); }

    CircularDeque<uint32_t> queue(32);
    queue.PushBack( si );
    searchInfo[si].state = ASTAR_QUEUEED;
    uint32_t ri = UINT32_MAX;
    while( !queue.Empty() && ri == UINT32_MAX  )
    {
        uint32_t ci = queue.Back();
        queue.PopBack();
        if( ci == ei )
        {
            ri = ei;
            AStarSearchNode& rin = searchInfo[ri];
            rin.state = ASTAR_PROCESSED;
            rin.heuristic = 0.0f;
        } else {
            SpaceNode* cn = root.nodes + ci;
            for( uint32_t i = 0; i < cn->connectionCnt; ++i )
            {
                uint32_t ccni = cn->connections[i];
                SpaceNode* ccn = root.nodes + ccni;
                if( lfunc(ccn) )
                {   
                    AStarSearchNode& ccsn = searchInfo[ccni];
                    if( ccsn.state == ASTAR_UNDEFINED )
                    {
                        ccsn.previous = ci;
                        ccsn.state = ASTAR_QUEUEED;
                        ccsn.heuristic = hfunc( ccn, e );
                        queue.PushBack( ccni );
                    }
                    if( queue.Size() > 1 )
                    {
                        uint32_t ni = queue.Size() - 1;
                        while( ni > 0 && searchInfo[queue[ni - 1]].heuristic < searchInfo[queue[ni]].heuristic )
                        {
                            uint32_t temp = queue[ni - 1];
                            queue[ni - 1] = queue[ni];
                            queue[ni] = temp;
                            ni -= 1;
                        }
                    }
                }
            }
        }
        searchInfo[ci].state = ASTAR_PROCESSED;
    }

    uint32_t cri = ri;
    while( cri != UINT32_MAX )
    {
        wps.Add(cri);
        cri = searchInfo[cri].previous;
    }
}